/********************************** (C) COPYRIGHT *******************************
* File Name          : foc.c
* Author             : nx666
* Version            : V1.0.0
* Date               : 2023/07/29
* Description        : 滑膜观测器接口这里实现接口.参考TI
*******************************************************************************/
/***********************************头文件区域***********************************/
#include "smo.h"
#include "IQmathLib.h"
#include <string.h>
#include "fiter.h"
#include "interact.h"

/***********************************类型定义*********************************/


/***********************************宏定义区***********************************/


/******************************** *全局变量区**********************************/

/*********************************************************************
 * @fn      getTheta
 *
 * @brief   获取 smo 预测的电角度，
 *              
 *             
 * @return  电角度
 *          
 */
static _iq getTheta(struct smo* v)
{
    // _iq temp;
    // temp = _IQtoIQ8(v->Theta) + IQ8_1_PI + 306;
    // if (temp >= IQ8_2_PI)
    // {
    //     temp -= IQ8_2_PI;
    // }
    // return temp;
    return _IQtoIQ8(v->Theta);
}

/*********************************************************************
 * @fn      getOmega
 *
 * @brief   获取 角速度 单位 弧度/s，
 *              
 *             
 * @return  电角速度
 *          
 */
static _iq getOmega(struct smo* v)
{ 
    return _IQtoIQ8(v->pll->omega);
}


/*********************************************************************
 * @fn      calc
 *
 * @brief   smo执行一次观测，
 *              Valpha  Vbeta 实际电压
 *              Ialpha  Ibeta 实际电流
 * @return  smo_ *，成功  NULL，失败
 *          
 */
static void calc(struct smo* v, _iq Valpha, _iq Vbeta, _iq Ialpha, _iq Ibeta)
{
     _iq E0;

    E0 = v->E0;

    //输入实际的alpha-beta轴的电压、电流
    v->Ialpha = _IQ8toIQ(Ialpha);
    v->Ibeta = _IQ8toIQ(Ibeta);
    v->Valpha = _IQ8toIQ(Valpha);
    v->Vbeta = _IQ8toIQ(Vbeta);

    // Sliding mode current observer
#if 1
    v->EstIalpha = _IQmpy(v->Fsmopos,v->EstIalpha) + _IQmpy(v->Gsmopos,(v->Valpha - v->Ealpha));
    v->EstIbeta = _IQmpy(v->Fsmopos1,v->EstIbeta) + _IQmpy(v->Gsmopos1,(v->Vbeta - v->Ebeta));
#else
    v->EstIalpha = _IQmpy(v->Fsmopos,v->EstIalpha) + _IQmpy(v->Gsmopos,(v->Valpha - 0));
    v->EstIbeta = _IQmpy(v->Fsmopos1,v->EstIbeta) + _IQmpy(v->Gsmopos1,(v->Vbeta - 0));
#endif
    // Current errors
    v->IalphaError = v->EstIalpha - v->Ialpha;  //得出观察与实际的误差
    v->IbetaError = v->EstIbeta - v->Ibeta;     //同上

   //Sliding control calculator
   if (v->IalphaError >= E0)          
   {
      v->Zalpha = E0;
   }
   else if (v->IalphaError <= -E0) 
   {
       v->Zalpha = -E0;
   }
   else 
   {
        v->Zalpha = v->IalphaError;
   }
   v->Zalpha = _IQmpy(v->Zalpha,v->Kslide);

   if (v->IbetaError >= E0)          
   {
      v->Zbeta = E0;
   }
   else if (v->IbetaError <= -E0) 
   {
       v->Zbeta = -E0;
   }
   else 
   {
        v->Zbeta = v->IbetaError;
   }
   v->Zbeta = _IQmpy(v->Zbeta,v->Kslide1);

    v->Ealpha = v->Zalpha;
    v->Ebeta = v->Zbeta;

#if 1
    v->EalphaLpf = _IQmpy(v->EalphaLpf,_IQ(1) - v->Kslf) + _IQmpy(v->Kslf,v->Ealpha); //v->Kslf滤波器增益
    v->EbetaLpf = _IQmpy(v->EbetaLpf,_IQ(1) - v->Kslf) + _IQmpy(v->Kslf,v->Ebeta);
#else
    v->EalphaLpf = v->fiterAlpha->update(v->fiterAlpha, v->Ealpha);
    v->EbetaLpf = v->fiterBeta->update(v->fiterBeta, v->Ebeta);
#endif
 
    //得出α与β轴的估算反电势就可以估算出角度了。
    v->pll->pll(v->pll, -v->EalphaLpf, v->EbetaLpf);

    v->Theta = v->pll->theta;
    v->Theta += (175 << 12);
    if (v->Theta > v->pll->thetaLimit)
    {
        v->Theta -= v->pll->thetaLimit;
    }
    else if (v->Theta < -v->pll->thetaLimit)
    {
        v->Theta += v->pll->thetaLimit;
    }

    // printMat[0] = (float)(v->Ibeta);
    // printMat[1] = (float)(v->EstIbeta);
    // printMat[2] = (float)(v->EbetaLpf);

    // printMat[2] = (float)(v->Theta);

    // printMat[0] = (float)(v->Theta);
    // printMat[1] = (float)(v->pll->omega);
    // printMat[2] = (float)(v->EbetaLpf);
    // printMat[2] = (float)(v->Ialpha);
    // printMat[3] = (float)(v->EstIalpha);

    // printData(printMat,3);
}


/*********************************************************************
 * @fn      smoCreate
 *
 * @brief   smo构造函数，
 *              void
 *
 * @return  smo_ *，成功  NULL，失败
 *          
 */
smo_ * smoCreate(void)
{
    smo_ *pSmo = NULL;

    pSmo = (smo_ *)malloc(sizeof(smo_));
    if (!pSmo)
    {
        return NULL;
    }
    memset((void *)pSmo, 0, sizeof(smo_));

    pSmo->pll = pllCreate();
    pSmo->fiterAlpha = create_movingAverage(SMO_FITER_WINDOW_SIZE);
    pSmo->fiterBeta = create_movingAverage(SMO_FITER_WINDOW_SIZE);

    // pFoc->foc_smopos->Fsmopos = _IQexp(_IQmpy(_IQdiv(_IQ(-FOC_MOTOR_RS) , _IQ(FOC_MOTOR_LS)),_IQ(FOC_SMOPOS_TS)));
    // pFoc->foc_smopos->Gsmopos = _IQmpy(_IQmpy(_IQdiv(_IQ(FOC_MOTOR_VB),_IQ(FOC_MOTOR_IB)), 
    //                                 (_IQdiv(_IQ(1),_IQ(FOC_MOTOR_RS)))), 
    //                                 (_IQ(1) - pFoc->foc_smopos->Fsmopos));
    // pFoc->foc_smopos->Kslide = _IQ(1);
    // PRINT_INFO("Fsmopos[%ld] Gsmopos[%ld]\n",pFoc->foc_smopos->Fsmopos,pFoc->foc_smopos->Gsmopos);

    //	v.Fsmopos = exp((-v.Rs/v.Ls)*(v.Ts));				
	//  v.Gsmopos = (v.Vb/v.Ib)*(1/v.Rs)*(1-v.Fsmopos);
    //pSmo->Fsmopos = _IQexp(_IQ(-(double)MOTOR_RS / MOTOR_LS * FOC_CALC_CYCLE / 1000));
    //pSmo->Gsmopos = _IQmpy(_IQ((double)MOTOR_VB / MOTOR_IB / MOTOR_RS * 1000),_IQ(1) - pSmo->Fsmopos);

    pSmo->Fsmopos = _IQ( 1 -(double)FOC_CALC_CYCLE * MOTOR_RS / MOTOR_LS / 1000 );
    pSmo->Gsmopos = _IQ( (double)FOC_CALC_CYCLE / MOTOR_LS );
    pSmo->Fsmopos1 = _IQ( 1 -(double)FOC_CALC_CYCLE * MOTOR_RS1 / MOTOR_LS1 / 1000 );
    pSmo->Gsmopos1 = _IQ( (double)FOC_CALC_CYCLE / MOTOR_LS1 );
    pSmo->Kslide = _IQ(SMO_KSLIDE);
    pSmo->Kslide1 = _IQ(SMO_KSLIDE1);
    pSmo->E0 = _IQ(SMO_E0);
    pSmo->Kslf = _IQ(SMO_KSLF);
    // pSmo->Fsmopos = _IQ(0.9683);  //Ts=140
    // pSmo->Gsmopos = _IQ(0.0583);   //Ts=140  / 1000

    pSmo->calc = calc;
    pSmo->getTheta = getTheta;
    pSmo->getOmega = getOmega;

    return pSmo;
}